//
// Created by yu on 2023/9/23.
//
#include "imuData.h"
#include <ros/ros.h>
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include <eigen3/Eigen/Dense>

// imu回调函数
void imuCallBack(const sensor_msgs::Imu &_datas)
{

    ImuData.quaX = _datas.orientation.x;
    ImuData.quaY = _datas.orientation.y;
    ImuData.quaZ = _datas.orientation.z;
    ImuData.quaW = _datas.orientation.w;

    ImuData.accelerate_x = _datas.linear_acceleration.x;
    ImuData.accelerate_y = _datas.linear_acceleration.y;
    ImuData.accelerate_z = _datas.linear_acceleration.z;

    ImuData.angular_velocity_x = _datas.angular_velocity.x;
    ImuData.angular_velocity_y = _datas.angular_velocity.y;
    ImuData.angular_velocity_z = _datas.angular_velocity.z;

    quaternionToEuler(ImuData);
}

// 磁力计回调函数
void magCallBack(const sensor_msgs::MagneticField &_datas)
{
    ImuData.mag_x = _datas.magnetic_field.x;
    ImuData.mag_y = _datas.magnetic_field.y;
    ImuData.mag_z = _datas.magnetic_field.z;
}

void quaternionToEuler(imuData data)
{

    Eigen::Quaterniond quaternion(data.quaW, data.quaX, data.quaY, data.quaZ);

    double rad = 180 / M_PI;

    ImuEuler.roll = rad * atan2(2 * (quaternion.w() * quaternion.x() + quaternion.y() * quaternion.z()),
                                1 - 2 * (pow(quaternion.x() + quaternion.y(), 2)));
    ImuEuler.pitch = rad * asin(2 * (quaternion.w() * quaternion.y() - quaternion.z() * quaternion.x()));
    ImuEuler.yaw = rad * atan2(2 * (quaternion.w() * quaternion.z() + quaternion.x() * quaternion.y()),
                               1 - 2 * (pow(quaternion.y() + quaternion.z(), 2)));

}